//Custom code
#include <stdio.h>
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "user_Serialdrv.h"
#include "custom.h"

//GENERAL NOTES: to do exponents, use pow() (possible math.pow?()). The syntax is pow(base, exponent). So 7 squared would be pow(7,2)
//for square root, use sqrt(). this may cause problems, as I have not included math.h. The issue is, of course, that there's not enough space for math.h
//possible workaround would be to define a function called sqare which takes an input and uses pow to return the square root of that input in int form. this only works if pow works without math.h, however



void doLights() {
	  //Pwm(1|2)_(green|red)
	  //Relay(1|2)_(green|red)
	  //Switch(1-3)_LED
	}

void joystickInfo() {
printf("p1_y-%d    p2_y-%d    p1_x-%d   p2_x-%d\r", p1_y, p2_y, p1_x, p2_x);
}//end of function


void runThrusters_arcade(unsigned char x_in, unsigned char y_in, unsigned char turbobutton, unsigned char* outFrontLeft, unsigned char* outFrontRight, unsigned char* outRearLeft, unsigned char* outRearRight) {
//take joystick unsigned chars and convert them to ints (don't forget to convert any outputs to unsigned chars before passing them to the victors!
//we have to use ints in order to use XOR operators (^)
signed int x = (x_in-127);
signed int y = (y_in-127);




//We will also be using signed ints for the motor outputs, then converting them at then end of the function to unsigned chars suitable for PWM generation
signed int outFL = 0;
signed int outFR = 0;
signed int outRL = 0;
signed int outRR = 0;

//below are setup variables which will be used to set the size of the joystick zones

//deadzone sets the radius of the circular dead zone from the center of the joystick
unsigned char deadzone = 20;

//slidezone sets the horizontal band in which the joystick will slide the robot sideways
unsigned char slidezone = 40;

//dead is used to check if the joystick is in the deadzone
unsigned char dead = 0; //we will multiply everything by dead to make sure that nothing happens if the joystick is in the deadzone.

//slide is used to check if the joystick is in the slidezone
unsigned char slide = 0;

//check if the joystick is in the square deadzone and set dead accordingly
//if ( (0-deadzone < x < 0+deadzone) && (0-deadzone < y < 0+deadzone) ) {dead = 0;}
//if ( (0-deadzone >= x <= 0+deadzone) && (0-deadzone >= x <= 0+deadzone) ) {dead = 1;}
dead = 0; //deadzone off for testing

Switch1_LED = dead; //Deadzone light


//check if the joystick is in the slidezone and set slide accordingly
if (0-slidezone > y < 0+slidezone) {slide = 1;}

Switch2_LED = slide; //slidezone light 

//----SLIDEZONE OPERATION CODE
if (slide) { 
	outFL = outRL = x*(y>0); //we multiply by y>0 to ensure that these won't go backwards if y is negative
	outFR = outRR = -1*x*(y<0); //we multiply by -1 to make sure these go forwards when y is negative, and by y<0 to ensure they won't go backwards if y is positive
	if (turbobutton) {
			outFL = outRL = x;
			outFR = outRR = -1*x;
		}//End turbo if
	}//end slide if
//----End slidezone code. make sure everything is sufficiently guarded so as to prevent other stuff from happening in the slidezone

//----QUADRANT 1 OPERATION CODE - currently commented out to isolate slidezone code for testing
if (x>0 && y>0) {
	outFL = 20; //Right now, testing needs to be done to determine if we can do square roots. Hence, we're just setting stuff to 20 for testing.
	if (turbobutton) {
		outRR = 20;
		}//end turbo if
	}//end slide if
//----End quadrant 1 code. 

//----QUADRANT 2 OPERATION CODE - currently commented out to isolate slidezone code for testing
if (x<0 && y>0) {
	outFR = 20; //Right now, testing needs to be done to determine if we can do square roots. Hence, we're just setting stuff to 20 for testing.
	if (turbobutton) {
		outRL = 20;
		}//end turbo if
	}//end slide if
//----End quadrant 2 code. 

//----QUADRANT 3 OPERATION CODE - currently commented out to isolate slidezone code for testing
if (x<0 && y<0) {
	outRL = 20; //Right now, testing needs to be done to determine if we can do square roots. Hence, we're just setting stuff to 20 for testing.
	if (turbobutton) {
		outFR = 20;
		}//end turbo if
	}//end slide if
//----End quadrant 3 code. 

//----QUADRANT 4 OPERATION CODE - currently commented out to isolate slidezone code for testing
if (x>0 && y<0) {
	outRR = 20; //Right now, testing needs to be done to determine if we can do square roots. Hence, we're just setting stuff to 20 for testing.
	if (turbobutton) {
		outFL = 20;
		}//end turbo if
	}//end slide if
//----End quadrant 4 code. 

//----FINAL CONVERSION CODE
//This is where we take our unsigned ints outFL, outFR, outRL, and outRR and convert them to unsigned chars before passing them to the victor outputs
*outFrontLeft = 127 + outFL*!dead;
*outFrontRight = 127 + outFR*!dead;
*outRearLeft = 127 + outRL*!dead; 
*outRearRight = 127 + outRR*!dead;
//OPTIONAL: if desired, these can be changed to include input_curve(127 + outXX). This may make for smoother sailing, or break everything
} //end of function




void runThrusters_tank(unsigned char x_Left_in, unsigned char y_Left_in, unsigned char x_Right_in, unsigned char y_Right_in, unsigned char turbobutton, unsigned char* outFrontLeft, unsigned char* outFrontRight, unsigned char* outRearLeft, unsigned char* outRearRight) {
//take joystick unsigned chars and convert them to ints (don't forget to convert any outputs to unsigned chars before passing them to the victors!
//we have to use ints in order to use XOR operators (^)
signed char x_Left = (x_Left_in-127);
signed char y_Left = (y_Left_in-127);
signed char x_Right = (x_Right_in-127);
signed char y_Right = (y_Right_in-127);

//We will also be using signed ints for the motor outputs, then converting them at then end of the function to unsigned chars suitable for PWM generation
signed char outFL = 0;
signed char outFR = 0;
signed char outRL = 0;
signed char outRR = 0;

//These will be used as intermediary variables for stage 1 and 2 conversion 


signed int outFL_raw = 0;
signed int outFR_raw = 0;
signed int outRL_raw = 0;
signed int outRR_raw = 0;

//----DEADZONE CODE
//deadzone sets the radius of the circular dead zone from the center of the joystick
unsigned char deadzone = 20;
//dead is used to check if the joystick is in the deadzone
unsigned char dead_Left = 0;
unsigned char dead_Right = 0;
//check if the joystick is in the square deadzone and set dead accordingly
if ( (0-deadzone < x_Left < 0+deadzone) && (0-deadzone < y_Left < 0+deadzone) ) {dead_Left = 1;}
if ( (0-deadzone >= x_Left <= 0+deadzone) && (0-deadzone >= x_Left <= 0+deadzone) ) {dead_Left = 0;}
if ( (0-deadzone < x_Right < 0+deadzone) && (0-deadzone < y_Right < 0+deadzone) ) {dead_Right = 1;}
if ( (0-deadzone >= x_Right <= 0+deadzone) && (0-deadzone >= x_Right <= 0+deadzone) ) {dead_Right = 0;}
Switch1_LED = dead_Left; //Deadzone light
Switch2_LED = dead_Right;

//--NORMAL OPERATION CODE
//This code is Overridden by slidezone operations
outFL = (y_Left)*(y_Left > 0); //if the left joystick is forwards, and the value for it is greater than 0, then run the front left thruster
//printf("y_Left = %d, outFL = %d, (y_Left > 0) = %d\r", y_Left, outFL, (y_Left > 0)); //diagnostics
outRL = -1*(y_Left)*(y_Left < 0); //do the opposite for the rear left motor
outFR = (y_Right)*(y_Right > 0); //do the above, for the right side.
outRR = -1*(y_Right)*(y_Right < 0); //you know what this does
//NOTE: This code will not work. It's close to what we want, but it will result in values higher than 127 and lower than -127. It also has some behavior issues.


//----FINAL CONVERSION CODE
//Stage 1 conversion: outXX now is adjusted for deadzones
outFL = outFL*!dead_Left;
outFR = outFR*!dead_Right;
outRL = outRL*!dead_Left;
outRR = outRR*!dead_Right;

//Stage 2 conversion: the difference is taken in the front and back, and then the appropriate thrusters are adjusted/canceled
//Front:
outFL_raw = (outFL - outFR)*(outFL > outFR);
outFR_raw = (outFR - outFL)*(outFR > outFL);
//Rear:
outRL_raw = (outRL - outRR)*(outRL > outRR);
outRR_raw = (outRR - outRL)*(outRR > outRL); 

//stage 3 conversion: we convert the signed chars into unsigned chars suitable to pass to the victors
*outFrontLeft = 127 + outFL_raw;
*outFrontRight = 127 + outFR_raw;
*outRearLeft = 127 + outRL_raw;
*outRearRight = 127 + outRR_raw;
//OPTIONAL: if desired, these can be changed to include input_curve(127 + outXX). This may make for smoother sailing, or break everything
} //end of function




void runBooster_tank(unsigned char y1_in, unsigned char y2_in, unsigned char* outBoost) {
//syntax: left x_in, right x_in, outFrontLeft, outFrontRight, outRearLeft, outRearRight
int totalY = 0; 
int averageY = 0;
int deadzone = 15;
char dead = 0;
totalY = (int)y1_in + (int)y2_in; //add the two. the (int) is a magical thing that makes stuff not screw up. I'm not really sure why it was screwing up in the first place.
averageY = totalY/2; //divide the total
// ---for deadzone
if(averageY > (127-deadzone) && averageY < (127+deadzone)) {
	dead = 1; //set dead to 1 if in deadzone.
	*outBoost = 127; //and turn off booster
} 
else
	*outBoost = averageY;  //run motor if in deadzone.


printf("totalY=%d    averageY=%d    dead=%d\r", totalY, averageY, dead);
//*outBoost = 127; FAILSAFE CODE IN COMMENTS
//if (y1_in > 157 && y2_in > 157) {*outBoost = 180;}
//if (y1_in < 97 && y2_in < 97) {*outBoost = 60;}

} //end of function

void slideThrusters_tank(unsigned char x_Left_in, unsigned char x_Right_in,  unsigned char* outFrontLeft, unsigned char* outFrontRight, unsigned char* outRearLeft, unsigned char* outRearRight) {
//take joystick unsigned chars and convert them to ints (don't forget to convert any outputs to unsigned chars before passing them to the victors!
//we have to use ints in order to use XOR operators (^)
signed char x_Left = (127 - x_Left_in);  //subtract 127 because x axis goes from 255 to 0 from left to right
signed char x_Right = (127 - x_Right_in);

int totalX = 0; 
int averageX = 0;
int finalX = 0;
char slideZone = 60; //slidezone width on either side of y axis
char inSlideZone = 0;
totalX = (int)x_Left + (int)x_Right; //add the two. the (int) is a magical thing that makes stuff not screw up. I'm not really sure why it was screwing up in the first place.
averageX = totalX/2; //divide the total

// ---for slideZone
if(averageX < -slideZone || averageX > slideZone) {inSlideZone = 1; } //set slideZone to zero if not in slideZone.
//slidezone chart below.
//  -127 ... sliding area ... char slideZone .. no slide .. y axis (0) .. no slide .. char slideZone ... sliding area ... 127

finalX = averageX * inSlideZone;

//debug
//printf("totalX=%d    averageX=%d    inSlideZone=%d\r", totalX, averageX, inSlideZone);

//do slide zone calculations if in slide zone
if(inSlideZone)
{
	//We will also be using signed ints for the motor outputs, then converting them at then end of the function to unsigned chars suitable for PWM generation
	signed char outFL = 0;
	signed char outFR = 0;
	signed char outRL = 0;
	signed char outRR = 0;

	//set motor values
	if(finalX < 0){  //if finalX is less than 0, go left by powering the right motors
		outFL = 0;
		outRL = 0;
		outFR = -finalX;  //opposite of finalX because it needs to be positive to make the motors go forwards.
		outRR = -finalX;
	}
	else if(finalX > 0){  //if finalX is greater than 0, go right by powering the left motors
		outFL = finalX;
		outRL = finalX;
		outFR = 0;
		outRR = 0;
	}
	
	
	//conversion: we convert the ints into unsigned chars suitable to pass to the victors
	*outFrontLeft = 127 + outFL;
	*outFrontRight = 127 + outFR;
	*outRearLeft = 127 + outRL;
	*outRearRight = 127 + outRR;
}//end if
	
}// end of function


void runBooster_arcade(unsigned char y_in, unsigned char* outBoost) {
char booster_deadzone = 20; //define vertical deadzone for booster

if ((127-booster_deadzone) > y_in || (127+booster_deadzone) < y_in) { //if the joystick is not within the deadzone, do some stuff
	*outBoost = input_curve(y_in);
		}// end of booster deadzone if 
Pwm1_green = (*outBoost > (127+booster_deadzone)); //run pwm1 oi green light
Pwm1_red = (*outBoost < (127-booster_deadzone)); //run pwm1 oi red light
	}// end of function




void doBallast(unsigned char up_button, unsigned char down_button, unsigned char trim_front_button, unsigned char trim_rear_button, unsigned char frontVent, unsigned char frontFill, unsigned char rearVent, unsigned char rearFill) {
	}





void runArm() {
	}





//old code is here for reference purposes
/*
void doDrive(unsigned char left, unsigned char right, unsigned char side, unsigned char* outLeft, unsigned char* outRight, unsigned char* outSide, unsigned char divideSwitch, unsigned char defaultDivide, unsigned char secondaryDivide) {
	char divideleft = 0;
	char divideright = 0;
	char divideside = 0;
	static int powersave = 0;
	static int waspressed = 0;
	static char divideby = 1;
	*outLeft = 127;
	*outRight = 127;
	*outSide = 127;
//Current args (p1_y, p2_y, p1_x, &pwm01, &pwm02, &pwm03, p3_sw_top, 1, 1)

//Toggle between powersave and normal operation
if (divideSwitch && !waspressed) {
	waspressed = 1;
	powersave = !powersave;
	}
if (!divideSwitch) {
	waspressed = 0;
	}


//Switch between powersave and normal mode. Also drive powersave LED
//Determine division factor by pressing a button. if nothing has been bressed since boot, divideby will be 1.
//
if (powersave) {
	divideby = secondaryDivide;
	rc_dig_out01 = 1;
	}
else {
	divideby = defaultDivide;
	rc_dig_out01 = 0;
	}


//Setup variables with negatives
divideleft = (input_curve(left)-127);
divideright = (input_curve(right)-127);
divideside = (input_curve(side)-127);

//Divide stuff (set defaultdivide to 1 to not divide when not in powersave mode)
	*outLeft = ((divideleft/divideby)+127);
	*outRight = ((divideright/divideby)+127);
		//Run Sideways thruster if trigger 1 is pressed
	if (p1_sw_trig) {
			*outSide = ((divideside/divideby)+127);
	}


---------------------------------------------------------------------
//We're not using this, unless we're really screwed
//Normal Operation: Trigger 2 is NOT pressed
//if (!p2_sw_trig) {
//	*outLeft = ((divideleft/divideby)+127);
//	*outRight = ((divideright/divideby)+127);
//if trigger one is also pressed, execute normal side-side operation
//	if (p1_sw_trig) {
//  		*outSide = (divideside/(divideby)+127);
//  	}
//	}
//
//Turbo Mode: Trigger 2 PRESSED
//if (p2_sw_trig) {
//  *outLeft = input_curve(left);
//  *outRight = input_curve(right);
//	if (p1_sw_trig) {
//  		*outSide = input_curve(side);
//  	}
//	}
//-------------------------------------------------------------------------

//END OF FUNCTION
} 


//BEGIN FAILSAFE DRIVECODE
void doDrive_failsafe(unsigned char left, unsigned char right, unsigned char side, unsigned char* outLeft, unsigned char* outRight, unsigned char* outSide) {
  char divideleft = 0;
  char divideright = 0;
  char divideside = 0;
  *outLeft = 127;
  *outRight = 127;
  *outSide = 127;
// This is the standard drive code, modified to accept another input and output. the extra input and output are used to regulate our side-side motor.



// Drive thrusters normally
	*outLeft = input_curve(left);
	*outRight = input_curve(right);
//Drive sideways thruster if left trigger is pressed
	if (p1_sw_trig) {
		*outSide = side;
	}
//END OF FUNCTION
}



//BETA FEATURE - NOT FINISHED (NOT CURRENTLY CALLED FROM MAIN)
void compTorque(unsigned char inV, unsigned char* outH) {
  int out = inV-127;
  out = out/4;
  out += 127;
  *outH = (unsigned char)(254-out); //Reverse?
} 
                                                 
//Single joystick code deleted, see archives if needed.

void doDepth(unsigned char joystick, unsigned char trigger, unsigned char* outDown, unsigned char incrementup, unsigned char incrementdown) {


  static unsigned char depth = 127;
  static int plus = 0;
  int plustemp = 0;

if (trigger) {
 depth = input_curve(joystick);
 plus = 0;
 plustemp = 0;
}
if (p3_sw_top) {
 depth = 0;
 plus = 0;
 plustemp = 0;
}
if ((p2_sw_aux1)&&(depth>1)) { 
depth ++;
}
if ((p2_sw_aux2)&&(depth<254)) {
depth --;
}
  *outDown = depth;
}


void handlePnu() {

//Function ends
}

void controlPump() {
 	char pumpOn = TRUE;

 	if (!rc_dig_in02) {
    	pumpOn = FALSE;
    	Switch2_LED = 1;
		rc_dig_out03 = 0;
  	}

rc_dig_out18 = pumpOn;   
	if (pumpOn) {
		rc_dig_out03 = 1;
	} 
}
*/


unsigned char input_curve(unsigned char _in) {

  unsigned char c_in;
  float in = (float)_in;
//   printf("%d\r",_in);
//   printf("%d\r",in);
  in = (in-127)*(in-127);
//   printf("%d\r",in);
  in = in / (float)127;
//   printf("%d\r",in);
  in += 127;
//   printf("%d\r",in);

  c_in = (unsigned char)in;
//   printf("%d\r",c_in);
//   printf("\r\r\r\r");

  if (_in < 127)
    c_in = 254-c_in;
  return c_in;
}
